
#include "mpu6050.h"

int16_t accelData[3] = {0};
int16_t gyroData[3] = {0};
int16_t tempData = 0;

void MPU6050_Init(void)
{
	MPU6050_WriteByte(0x6B,0x00);
	MPU6050_WriteByte(0x19,0x07);
	MPU6050_WriteByte(0x1A,0x04);
	MPU6050_WriteByte(0x1C,0x01);
	MPU6050_WriteByte(0x1B,0x18);
	
}

uint8_t MPU6050_ReadByte(uint8_t reg)
{
	uint8_t data = 0;
	HAL_I2C_Mem_Read(&hi2c1,MPU_ADD_READ,reg,I2C_MEMADD_SIZE_8BIT,&data,1,0xff);
	return data;
}

void MPU6050_WriteByte(uint8_t reg,uint8_t data)
{
	HAL_I2C_Mem_Write(&hi2c1,MPU_ADD_WRITE,reg,I2C_MEMADD_SIZE_8BIT,&data,1,0xff);
}

void MPU6050_ReadGyro(void)
{
	gyroData[0] = (MPU6050_ReadByte(MPU6050_GYRO_XOUT_H) << 8) | MPU6050_ReadByte(MPU6050_GYRO_XOUT_L);
	gyroData[1] = (MPU6050_ReadByte(MPU6050_GYRO_YOUT_H) << 8) | MPU6050_ReadByte(MPU6050_GYRO_YOUT_L);
	gyroData[2] = (MPU6050_ReadByte(MPU6050_GYRO_ZOUT_H) << 8) | MPU6050_ReadByte(MPU6050_GYRO_ZOUT_L);
}

void MPU6050_ReadAccel(void)
{
	accelData[0] = (MPU6050_ReadByte(MPU6050_ACCEL_XOUT_H) << 8) | MPU6050_ReadByte(MPU6050_ACCEL_XOUT_L);
	accelData[1] = (MPU6050_ReadByte(MPU6050_ACCEL_YOUT_H) << 8) | MPU6050_ReadByte(MPU6050_ACCEL_YOUT_L);
	accelData[2] = (MPU6050_ReadByte(MPU6050_ACCEL_ZOUT_H) << 8) | MPU6050_ReadByte(MPU6050_ACCEL_ZOUT_L);
}

void MPU6050_ReadTemperature(void)
{
	tempData = (MPU6050_ReadByte(MPU6050_TEMP_OUT_H) << 8) | MPU6050_ReadByte(MPU6050_TEMP_OUT_L);
}

